#include "CollisionDetectionOnConvexDecomp.h"

#include "btBulletCollisionCommon.h"
#include "LinearMath/btIDebugDraw.h"
#include "GL_ShapeDrawer.h"
#include "GlutStuff.h"
#include "GLDebugDrawer.h"

//included for convex decomposition--------------------------------------
#include "hacdCircularList.h"
#include "hacdVector.h"
#include "hacdICHull.h"
#include "hacdGraph.h"
#include "hacdHACD.h"

#include "cd_wavefront.h"
#include "ConvexBuilder.h"

#include "LinearMath/btQuickprof.h"
#include "LinearMath/btGeometryUtil.h"
//-----------------------------------------------------------------------

btVector3	centroid=btVector3(0,0,0); //for new decomposed shape from HACD

btScalar yaw=0.f,pitch=0.f,roll=0.f;
const int maxNumObjects = 4;
const int numObjects = 2;
btCollisionObject	objects[maxNumObjects];
btCollisionWorld*	collisionWorld = 0;
GLDebugDrawer debugDrawer;
double movingSpeed = 0.001;

//#define TESTCASE_1
#define TESTCASE_2
//#define DEBUG_DRAW

CollisionDetectionOnConvexDecomp::CollisionDetectionOnConvexDecomp(void)
{
}

CollisionDetectionOnConvexDecomp::~CollisionDetectionOnConvexDecomp(void)
{
}

void	CollisionDetectionOnConvexDecomp::initPhysics()
{
#ifdef DEBUG_DRAW		
	m_debugMode |= btIDebugDraw::DBG_DrawWireframe;
#endif
	
	btMatrix3x3 basisA;
	basisA.setIdentity();

	btMatrix3x3 basisB;
	basisB.setIdentity();

	objects[0].getWorldTransform().setBasis(basisA);
	objects[1].getWorldTransform().setBasis(basisB);

#ifdef TESTCASE_1
	//setup test case 1 between two cubes on left and right sides of x axis.
	objects[1].getWorldTransform().setOrigin(btVector3(-2, 0, 0));
	objects[1].getWorldTransform().setOrigin(btVector3(2, 0, 0));

	btBoxShape* cubeLeftMoving = new btBoxShape(btVector3(1, 1, 1)); //cube of 2, 2 ,2
	cubeLeftMoving->setMargin(0.0f);

	btBoxShape* cubeRightStatic = new btBoxShape(btVector3(1, 1, 1)); //cube of 2, 2 ,2 
	cubeRightStatic->setMargin(0.0f);

	//ConvexHullShape	hullA(points0,3);
	//hullA.setLocalScaling(btVector3(3,3,3));
	//ConvexHullShape	hullB(points1,4);
	//hullB.setLocalScaling(btVector3(4,4,4));

	objects[0].setCollisionShape(cubeLeftMoving);//set the bounding volume, aka, collision shape
	objects[1].setCollisionShape(cubeRightStatic); //can also hullA or hullB as collision shape
#endif

#ifdef TESTCASE_2
	
	setCameraDistance(3000.f);
	ConvexDecomposition::WavefrontObj wo;
	unsigned int tcount = 0; //number of triangles from the obj file
	//tcount = wo.loadObj("boat.obj");
	tcount = wo.loadObj("lod0.obj");

	btCompoundShape* compound = 0;

#pragma region CONVEXDECOMPRESULTCOLLECTION
	class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
	{
		public:
			int   	mBaseCount;
  			int		mHullCount;

			btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
			btAlignedObjectArray<btVector3> m_convexCentroids;

			MyConvexDecomposition () : mBaseCount(0), mHullCount(0)
			{

			}

			virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
			{
				btTriangleMesh* trimesh = new btTriangleMesh();
				btVector3 localScaling(6.f,6.f,6.f);

				//calc centroid, to shift vertices around center of mass
				centroid.setValue(0,0,0);

				btAlignedObjectArray<btVector3> vertices;

				for (unsigned int i=0; i<result.mHullVcount; i++)
				{
					btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
					vertex *= localScaling;
					centroid += vertex;		
				}

				centroid *= 1.f/(float(result.mHullVcount) );

				//const unsigned int *src = result.mHullIndices;
				for (unsigned int i=0; i<result.mHullVcount; i++)
				{
					btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
					vertex *= localScaling;
					vertex -= centroid ;
					vertices.push_back(vertex);
				}

				const unsigned int *src = result.mHullIndices;
				for (unsigned int i=0; i<result.mHullTcount; i++)
				{
					unsigned int index0 = *src++;
					unsigned int index1 = *src++;
					unsigned int index2 = *src++;

					btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
					btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
					btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
					vertex0 *= localScaling;
					vertex1 *= localScaling;
					vertex2 *= localScaling;
							
					vertex0 -= centroid;
					vertex1 -= centroid;
					vertex2 -= centroid;

					trimesh->addTriangle(vertex0,vertex1,vertex2);

					index0+=mBaseCount;
					index1+=mBaseCount;
					index2+=mBaseCount;
				}
				
				btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());

				convexShape->setMargin(0.01f);
				m_convexShapes.push_back(convexShape);
				m_convexCentroids.push_back(centroid);
				mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
		}
};	

#pragma endregion CONVEXDECOMPRESULTCOLLECTION

	if (tcount)
	{

#pragma region SETUPCONVEXDECOMPOSITION

	unsigned int depth = 5;
	float cpercent     = 5;
	float ppercent     = 15;
	unsigned int maxv  = 16;
	float skinWidth    = 0.0;

	printf("WavefrontObj num triangles read %i\n",tcount);
	ConvexDecomposition::DecompDesc desc;
	desc.mVcount       = wo.mVertexCount;
	desc.mVertices     = wo.mVertices;
	desc.mTcount       = wo.mTriCount;
	desc.mIndices      = (unsigned int *)wo.mIndices;
	desc.mDepth        = depth;
	desc.mCpercent     = cpercent;
	desc.mPpercent     = ppercent;
	desc.mMaxVertices  = maxv;
	desc.mSkinWidth    = skinWidth;

	MyConvexDecomposition	convexDecomposition;
	desc.mCallback = &convexDecomposition;

#pragma endregion SETUPCONVEXDECOMPOSITION

#pragma region PERFORMHACD

	std::vector< HACD::Vec3<HACD::Real> > points;
	std::vector< HACD::Vec3<long> > triangles;

	for(int i=0; i<wo.mVertexCount; i++ ) 
	{
		int index = i*3;
		HACD::Vec3<HACD::Real> vertex(wo.mVertices[index], wo.mVertices[index+1],wo.mVertices[index+2]);
		points.push_back(vertex);
	}

	for(int i=0;i<wo.mTriCount;i++)
	{
		int index = i*3;
		HACD::Vec3<long> triangle(wo.mIndices[index], wo.mIndices[index+1], wo.mIndices[index+2]);
		triangles.push_back(triangle);
	}


	HACD::HACD myHACD;
	myHACD.SetPoints(&points[0]);
	myHACD.SetNPoints(points.size());
	myHACD.SetTriangles(&triangles[0]);
	myHACD.SetNTriangles(triangles.size());
	myHACD.SetCompacityWeight(0.1);
	myHACD.SetVolumeWeight(0.0);

	// HACD parameters
	// Recommended parameters: 2 100 0 0 0 0
	size_t nClusters = 2;
	double concavity = 100;
	bool invert = false;
	bool addExtraDistPoints = false;
	bool addNeighboursDistPoints = false;
	bool addFacesPoints = false;       

	myHACD.SetNClusters(nClusters);                     // minimum number of clusters
	myHACD.SetNVerticesPerCH(100);                      // max of 100 vertices per convex-hull
	myHACD.SetConcavity(concavity);                     // maximum concavity
	myHACD.SetAddExtraDistPoints(addExtraDistPoints);   
	myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);   
	myHACD.SetAddFacesPoints(addFacesPoints); 

	myHACD.Compute();
	nClusters = myHACD.GetNClusters();	

	myHACD.Save("output.wrl", false);

#pragma endregion PERFORMHACD

#pragma region CREATETHECOMPOUNDSHAPE

	if (1)
	{
		//btCompoundShape* compound = new btCompoundShape();
		compound = new btCompoundShape();

		btTransform trans;
		trans.setIdentity();

		for (int c=0;c<nClusters;c++)
		{
			//generate convex result
			size_t nPoints = myHACD.GetNPointsCH(c);
			size_t nTriangles = myHACD.GetNTrianglesCH(c);

			float* vertices = new float[nPoints*3];
			unsigned int* triangles = new unsigned int[nTriangles*3];
				
			HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
			HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
			myHACD.GetCH(c, pointsCH, trianglesCH);

			// points
			for(size_t v = 0; v < nPoints; v++)
			{
				vertices[3*v] = pointsCH[v].X();
				vertices[3*v+1] = pointsCH[v].Y();
				vertices[3*v+2] = pointsCH[v].Z();
			}
			// triangles
			for(size_t f = 0; f < nTriangles; f++)
			{
				triangles[3*f] = trianglesCH[f].X();
				triangles[3*f+1] = trianglesCH[f].Y();
				triangles[3*f+2] = trianglesCH[f].Z();
			}

			delete [] pointsCH;
			delete [] trianglesCH;

			ConvexResult r(nPoints, vertices, nTriangles, triangles);
			convexDecomposition.ConvexDecompResult(r);
		}

		for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
		{
			btVector3 centroid = convexDecomposition.m_convexCentroids[i];
			trans.setOrigin(centroid);
			btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
			compound->addChildShape(trans,convexShape); 								
		}
	}
	}
#pragma endregion CREATETHECOMPOUNDSHAPE

	//setup test case 1 between two cubes on left and right sides of x axis.
	objects[0].getWorldTransform().setOrigin(btVector3(-2500, 0, 0));
	objects[1].getWorldTransform().setOrigin(btVector3(1000, 0, 0));

	objects[0].setCollisionShape(compound);//set the bounding volume, aka, collision shape
	objects[1].setCollisionShape(compound); //can also hullA or hullB as collision shape
#endif

	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
	
//below using sweep and prune broadphase, thus the world dimension must be known. 
//we can also choose to use dynamic AABB tree
	btVector3	worldAabbMin(-1000,-1000,-1000);
	btVector3	worldAabbMax(1000,1000,1000);
	btAxisSweep3*	broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
	
	collisionWorld = new btCollisionWorld(dispatcher,broadphase,collisionConfiguration);
	collisionWorld->setDebugDrawer(&debugDrawer);
		
	collisionWorld->addCollisionObject(&objects[0]);
	collisionWorld->addCollisionObject(&objects[1]);
}

void CollisionDetectionOnConvexDecomp::clientMoveAndDisplay()
{
	displayCallback();
}

struct btDrawingResult : public btCollisionWorld::ContactResultCallback
{
	virtual	btScalar	addSingleResult(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
	{
		std::cout	<< "collision happens, contact points are: "	
					<< cp.getPositionWorldOnA().x()
					<< ", "
					<< cp.getPositionWorldOnA().y()
					<< ", "
					<< cp.getPositionWorldOnA().z()
					<< " on one object and "
					<< cp.getPositionWorldOnB().x()
					<< ", "
					<< cp.getPositionWorldOnB().y()
					<< ", "
					<< cp.getPositionWorldOnB().z()
					<< " on the other object."
					<< std::endl;
		
		movingSpeed = 0;
		return 0;
	}
};

void CollisionDetectionOnConvexDecomp::displayCallback(void)
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	glDisable(GL_LIGHTING);
		btScalar m[16];
	
	btVector3	worldBoundsMin,worldBoundsMax;
	collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);

	for (int i=0;i<numObjects;i++)
	{
		objects[i].getWorldTransform().getOpenGLMatrix( m );
		m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(0.f,0.f,1.f),getDebugMode(),worldBoundsMin,worldBoundsMax);
	}

	collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
	
	if (collisionWorld)
		collisionWorld->performDiscreteCollisionDetection();
#ifdef DEBUG_DRAW		
	glDisable(GL_TEXTURE_2D);
	for (int i=0;i<numObjects;i++)
	{
		collisionWorld->debugDrawObject(objects[i].getWorldTransform(),objects[i].getCollisionShape(), btVector3(1,0,0));
	}
#endif

	btDrawingResult renderCallback;

	//collisionWorld->contactPairTest(&objects[0],&objects[1], renderCallback);
	collisionWorld->contactTest(&objects[0],renderCallback);

	if (!m_idle)
	{
		btScalar timeInSeconds = getDeltaTimeMicroseconds()/1000.f;

#ifdef TESTCASE_1
		//below translate object[0] towards object[1] along x-axis so that these two can collide with each other
		objects[0].getWorldTransform().setOrigin(objects[0].getWorldTransform().getOrigin()+btVector3(movingSpeed*timeInSeconds, 0, 0));
#endif
#ifdef TESTCASE_2
		objects[0].getWorldTransform().setOrigin(objects[0].getWorldTransform().getOrigin()+btVector3(400*movingSpeed*timeInSeconds, 0, 0));
#endif
	}

	glFlush();
    swapBuffers();
}

void CollisionDetectionOnConvexDecomp::clientResetScene()
{
#ifdef TESTCASE_1
	objects[0].getWorldTransform().setOrigin(btVector3(-2.0f,0.0f,0.0f));
	objects[1].getWorldTransform().setOrigin(btVector3(2.0f,0.0f,0.0f));

	objects[0].getWorldTransform().getBasis().getEulerYPR(yaw,pitch,roll);
	pitch = 3.14159265f/4.f;
	yaw = 3.14159265f/4.f;
	objects[0].getWorldTransform().getBasis().setEulerYPR(yaw,pitch,roll);
#endif

#ifdef TESTCASE_2
	objects[0].getWorldTransform().setOrigin(btVector3(-2500, 0, 0));
	objects[1].getWorldTransform().setOrigin(btVector3(1000, 0, 0));

	objects[1].getWorldTransform().getBasis().getEulerYPR(yaw,pitch,roll);
	pitch = 3.14159265f/2.f;
	yaw = 3.14159265f/2.f;
	objects[1].getWorldTransform().getBasis().setEulerYPR(yaw,pitch,roll);
#endif
}


